/*
 *
 * Copyright (C) 2014
 * Julio Jarquin, Gustavo Arechavaleta <garechav@cinvestav.edu.mx>
 * CINVESTAV - Saltillo Campus
 *
 * This file is part of HRLocomotion-1.0
 * HRLocomotion-1.0 free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * HRLocomotion-1.0 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301  USA
 */

/**
 *	\file src/robot_system.cc
 *	\author Julio Jarquin, Gustavo Arechavaleta
 *	\version 1.0
 *	\date 2015
 *
 *	Implementation of the RobotSystem class.
 */

#include "HRLocomotion/robot_system.h"
#include <iostream>

namespace hrLocomotion{

RobotSystem::RobotSystem():g(9.806){

}

void RobotSystem::init(const real_t & h, const real_t & T){
    T_s = T;
    x_k.setZero(3,1);
    y_k.setZero(3,1);
    theta_k.setZero(3,1);

    A_s.resize(3,3);
    A_s << 1 , T , T*T/2.0 , 0 , 1 , T , 0 , 0 , 1;
    B_s.resize(3,1);
    B_s << T*T*T/6.0 , T*T/2.0 , T ;
    C_s.resize(1,3);
    C_s << 1 , 0 , -h/g;
}

void RobotSystem::nextState(const Vector3r & jerkXYTheta, RobotState & robotState){
    using std::cout;
    using std::endl;
    x_k = A_s*robotState.getX() + B_s*jerkXYTheta(0);
    y_k = A_s*robotState.getY() + B_s*jerkXYTheta(1);
    theta_k = A_s*robotState.getTheta()+B_s*jerkXYTheta(2);
    robotState.setState(x_k,y_k,theta_k);
    zx_k = C_s*robotState.getX();  //Calculate x^zmp_k+1  with x_k+1  TODO:Change vectorXr to scalar
    zy_k = C_s*robotState.getY();  //Calculate y^zmp_k+1  with y_k+1  TODO:Change vectorXr to scalar
}


/*
void RobotSystem::init(const real_t & T,const real_t & h, VectorXr x, VectorXr y){
    T_s = T;
    x_k = x;
    y_k = y;
    theta_k.setZero(3,1);
    A_s.resize(3,3);
    A_s << 1 , T , T*T/2.0 , 0 , 1 , T , 0 , 0 , 1;
    B_s.resize(3,1);
    B_s << T*T*T/6.0 , T*T/2.0 , T ;
    C_s.resize(1,3);
    C_s << 1 , 0 , -h/g;

}

void RobotSystem::init(const real_t & T,const real_t & h, VectorXr x, VectorXr y, VectorXr theta){
    T_s = T;
    x_k = x;
    y_k = y;
    theta_k = theta;

    A_s.resize(3,3);
    A_s << 1 , T , T*T/2.0 , 0 , 1 , T , 0 , 0 , 1;
    B_s.resize(3,1);
    B_s << T*T*T/6.0 , T*T/2.0 , T ;
    C_s.resize(1,3);
    C_s << 1 , 0 , -h/g;
}

void RobotSystem::nextState(VectorXr ux, VectorXr uy){
    x_k = A_s*x_k + B_s*ux;
    y_k = A_s*x_k + B_s*uy;

    zx_k = C_s*ux;
    zy_k = C_s*uy;
}

void RobotSystem::nextState(VectorXr ux, VectorXr uy, VectorXr utheta){
    x_k = A_s*x_k + B_s*ux;
    y_k = A_s*x_k + B_s*uy;
    theta_k = A_s*x_k+B_s*utheta;

    zx_k = C_s*ux;
    zy_k = C_s*uy;
}


void RobotSystem::setState(VectorXr x, VectorXr y){
    x_k = x;
    y_k = y;
}

void RobotSystem::setState(VectorXr x, VectorXr y, VectorXr theta){
    x_k = x;
    y_k = y;
    theta_k = theta;
}
*/

}
